#include "rtrobot.h"
#include "log/zuclog.h"
#include "motion/modbuscrc.h"
#include "string.h"
#include "rtdev/servo/jkservo.h"

#include <math.h>

#define ROBOT_DEBUG(fmt, ...) zuclog_debug("RTROBOT", fmt, ##__VA_ARGS__)
#define ROBOT_INFO(fmt, ...) zuclog_info("RTROBOT", fmt, ##__VA_ARGS__)
#define ROBOT_ERROR(fmt, ...) zuclog_error("RTROBOT", fmt, ##__VA_ARGS__)

const uint16_t SDO_INDEX_ROB_DH_ENABLE = 1005;
const uint16_t SDO_INDEX_ROB_DH_VERSION = 9201;
const uint16_t SDO_INDEX_ROB_DH_CRC = 9200;
const uint16_t SDO_INDEX_ROB_DH_A = 9202;
const uint16_t SDO_INDEX_ROB_DH_ALPHA = 9203;
const uint16_t SDO_INDEX_ROB_DH_D = 9204;
const uint16_t SDO_INDEX_ROB_DH_THETA = 9205;
const uint16_t SDO_INDEX_SERVO_VERSION = 9007;
const uint16_t SDO_INDEX_SERVO_FIRST_POWERON_DATE = 9013;
const uint16_t SDO_INDEX_SERVO_HOME_VAL = 2215;
const uint16_t SDO_INDEX_SERVO_MIN_POS = 2233;          //set
const uint16_t SDO_INDEX_SERVO_MAX_POS = 2232;          //set
const uint16_t SDO_INDEX_SERVO_BRAKE = 2240;            //set
const uint16_t SDO_INDEX_SERVO_MULTI_TURN_STEP = 2241;  //set
const uint16_t SDO_INDEX_SERVO_TORQ_UNIT = 9901;        //set
const uint16_t SDO_INDEX_SERVO_COLLISION_BASE = 2245;
const uint16_t SDO_INDEX_SERVO_COLLISION_TORQ = 2246;
const uint16_t SDO_INDEX_ROB_SERIALNUM = 9009;
const uint16_t SDO_INDEX_SERVO_MOTOR_SUPPLIER = 2000;
const uint16_t SDO_INDEX_SERVO_TEMPERATURE = 9108;
const uint16_t SDO_INDEX_SERVO_AVERAGE_POWER = 9101;
const uint16_t SDO_INDEX_SERVO_FLUCTUATION = 9102;
const uint16_t SDO_INDEX_SERVO_TOTAL_RUN_LAPS = 9103;
const uint16_t SDO_INDEX_SERVO_TOTAL_RUN_TIME = 9104;
const uint16_t SDO_INDEX_SERVO_CUR_RUN_LAPS = 9105;
const uint16_t SDO_INDEX_SERVO_CUR_RUN_TIME = 9106;
const uint16_t SDO_INDEX_SERVO_CAB_TYPE = 9011;            //set
const uint16_t SDO_INDEX_SERVO_BACK_DRIVE_ENABLE = 1017;   //set
const uint16_t SDO_INDEX_SERVO_AVOID_POS_MUTATION = 1018;  //set

// 1. 查询伺服版本号，2. 查询机器人序列号

void rtdev::RtRobot::do_robot_query_setting_when_active()
{
    ROBOT_INFO("do_robot_query_setting_when_active");
    query_robot_joint_version();  // 查询伺服版本號
    query_robot_serial_num();     // 查询一关节序列号
    query_dh_param();
    query_dyn_param();

    query_robot_joint_first_poweron_date();
    query_robot_joint_motor_supplier_id();
    query_robot_joint_collision_setting();   // 查询机型碰撞阈值设置
    set_robot_joint_torque_command_unit(2);  // 设置力矩单位
    set_robot_joint_collision_sensitivity(local_rtrobot_status_.robot_safety_para.clsn_sensitivity);
    for (size_t joint_num = 0; joint_num < joint_servo_.size(); joint_num++)
    {  // 设置拖拽软限位
        // set_robot_joint_min_limit(joint_num, joint_para_[joint_num].min_pos_limit);
        // set_robot_joint_max_limit(joint_num, joint_para_[joint_num].max_pos_limit);
    }
}

void rtdev::RtRobot::reset_servo() { joint_servo_.clear(); }

int rtdev::RtRobot::query_robot_serial_num()
{
    if (joint_servo_.size() == 0)
    {
        local_rtrobot_status_.serial_num_ = 0;
        return -1;
    }
    // 查询机型序列号
    int err = joint_servo_[0]->get_sdo_blocking(SERVO_OBJID_SERIAL_NUM, &local_rtrobot_status_.serial_num_, 4, 40);
    if (err)
    {
        ROBOT_ERROR("query_robot_serial_num error, %d\n", err);
        return -1;
    }
    update_robot_type();
    return 0;
}

int rtdev::RtRobot::sync_robot_position(const int* pos, int interval_us)
{
    int ret = 0;
    for (size_t i = 0; i < joint_servo_.size(); i++) { ret |= joint_servo_[i]->set_position(pos[i]); }
    return ret;
}

int rtdev::RtRobot::sync_robot_torque(const int* torque, const int* grvcomp)
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++) { ret |= joint_servo_[i]->sync_torque(torque[i], grvcomp[i]); }
    return ret;
}

int rtdev::RtRobot::set_robot_torque_forward_enable(uint8_t enable)
{
    ROBOT_DEBUG("set_robot_torque_forward_enable %d\n", enable);
    // for (size_t i = 0; i < joint_servo_.size(); i++) { joint_servo_[i]->set_dyn_forward(enable); }
    return 0;
}

void rtdev::RtRobot::update_robot_type() { robot_type_ = RobotType(local_rtrobot_status_.serial_num_ / 100000000); }

void rtdev::RtRobot::check_dyn_param() {}

int rtdev::RtRobot::query_dh_param()
{
    if (joint_servo_.size() == 0)
    {
        local_rtrobot_status_.dh.state = DHYN_NOT_USED;
        return -1;
    }
    for (size_t i = 0; i < joint_servo_.size(); i++)
    {
        if (joint_servo_[i]->is_sim())
        {
            local_rtrobot_status_.dh.state = DHYN_NOT_USED;
            return 0;
        }
    }

    // enable
    local_rtrobot_status_.dh.state = DHYN_IN_READING;
    int32_t enable = 0;
    if (joint_servo_[0]->get_sdo_blocking(SDO_INDEX_ROB_DH_ENABLE, &enable, 4, 40))
    {
        ROBOT_ERROR("query_dh_param error, %d\n", -1);
        local_rtrobot_status_.dh.state = DHYN_READ_ERR;
        return -1;
    }
    local_rtrobot_status_.dh.enable = enable == ROBOT_DH_ENABLED_FLAG;
    ROBOT_DEBUG("get Robot DH compensate is %s", local_rtrobot_status_.dh.enable ? "active" : "deactive");
    if (!local_rtrobot_status_.dh.enable)
    {
        local_rtrobot_status_.dh.state = DHYN_NOT_USED;
        return 0;
    }

    // go on read
    //version && crc
    if (joint_servo_[0]->get_sdo_blocking(SDO_INDEX_ROB_DH_VERSION, &local_rtrobot_status_.dh.version, 4, 40) ||
        joint_servo_[0]->get_sdo_blocking(SDO_INDEX_ROB_DH_CRC, &local_rtrobot_status_.dh.crc, 4, 40))
    {
        ROBOT_ERROR("query_dh_param error, %d\n", -1);
        local_rtrobot_status_.dh.state = DHYN_READ_ERR;
        return -1;
    }

    for (size_t i = 0; i < joint_servo_.size(); i++)
    {
        if (joint_servo_[i]->get_sdo_blocking(SDO_INDEX_ROB_DH_A, &local_rtrobot_status_.dh.joint[i].a, 4, 40) ||
            joint_servo_[i]->get_sdo_blocking(SDO_INDEX_ROB_DH_ALPHA, &local_rtrobot_status_.dh.joint[i].alpha, 4, 40) ||
            joint_servo_[i]->get_sdo_blocking(SDO_INDEX_ROB_DH_D, &local_rtrobot_status_.dh.joint[i].d, 4, 40) ||
            joint_servo_[i]->get_sdo_blocking(SDO_INDEX_ROB_DH_THETA, &local_rtrobot_status_.dh.joint[i].theta, 4, 40))
        {
            ROBOT_ERROR("query_dh_param error, %d\n", -1);
            local_rtrobot_status_.dh.state = DHYN_READ_ERR;
            return -1;
        }
    }
    local_rtrobot_status_.dh.state = DHYN_READ_OVER;
    return 0;
}

int rtdev::RtRobot::query_dyn_param() { return 0; }

void rtdev::RtRobot::period_query_joint()
{
    if (period_query_enable_)
    {
        if (period_query_count_ < PERIOD_QUERY_ALL_INTERAL)
        {
            ++period_query_count_;
        }
        else
        {
            period_query_count_++;
            if (period_query_count_ % 2 == 0)
            {
                query_robot_joint_temperature();
                query_robot_joint_voltage();
                query_robot_joint_average_power();
                query_robot_joint_current_fluctuation();
            }
            else
            {
                query_robot_joint_sum_running_circles();
                query_robot_joint_sum_running_time();
                query_robot_joint_cur_running_circles();
                query_robot_joint_cur_running_time();
                period_query_count_ = 0;
            }
        }
    }
    else
    {
        period_query_count_ = 0;
    }
}

void rtdev::RtRobot::set_robot_series_joint_cfg(uint8_t series, const int32_t* offset, const int* dir)
{
    robot_type_ = RobotType(series);
    ROBOT_DEBUG("set robot type to %d from task\n", robot_type_);
    // for (size_t i = 0; i < joint_servo_.size(); i++) { joint_servo_[i]->config_offset_dir(offset[i], dir[i]); }
}

int rtdev::RtRobot::set_robot_joint_collision_sensitivity(uint8_t sensitivity)
{
    local_rtrobot_status_.robot_safety_para.clsn_sensitivity = sensitivity;

    // for (size_t i = 0; i < joint_servo_.size(); i++) { joint_servo_[i]->set_collision_level(sensitivity); }
    return 0;
}

int rtdev::RtRobot::set_robot_joint_friction_compensate(uint8_t joint, int friciotn)
{
    // return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_FRICTION, &friciotn, [joint](uint8_t* rx_data, int s) -> int {
    //     if (s != 7)
    //         return s;

    //     return 0;
    // });
    return 0;
}

int rtdev::RtRobot::set_robot_joint_brake_lease(uint8_t joint, int relese)
{
    // return joint_servo_[joint]->set_sdo(SERVO_OBJID_BREAK_RELEASE, &relese, [](uint8_t* rx_data, int s) -> int {
    //     if (s != 7)
    //         return s;
    //     return 0;
    // });
    return 0;
}

int rtdev::RtRobot::set_robot_joint_errpos(uint8_t joint, double errpos)
{
    // int err = (int)errpos;
    // return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_WARNING_DEVIATION, &err, [](uint8_t* rx_data, int s) -> int {
    //     if (s != 7)
    //         return s;
    //     return 0;
    // });
    return 0;
}

int rtdev::RtRobot::set_robot_joint_min_limit(uint8_t joint, double joint_min_limit)
{
    // joint_para_[joint].min_pos_limit = joint_min_limit;
    // int data = (int)ceil(joint_min_limit);

    // if (robot_type_ == ROBOT_TYPE_AT)
    // {
    //     int dir = joint_servo_[joint]->get_dir();
    //     data = data * dir + joint_servo_[joint]->get_zero_offset();
    //     // 7轴机型 限位处理 不放在伺服；由控制器完成
    //     if (dir == -1)
    //     {
    //         return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_MAX_LIMIT, &data, [joint](uint8_t* rx_data, int s) -> int {
    //             if (s != 7)
    //                 return s;

    //             return 0;
    //         });
    //     }
    //     else
    //     {
    //         return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_MIN_LIMIT, &data, [](uint8_t* rx_data, int s) -> int {
    //             if (s != 7)
    //                 return s;

    //             return 0;
    //         });
    //     }
    // }
    // else
    // {
    //     return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_MIN_LIMIT, &data, [](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;

    //         return 0;
    //     });
    // }
    return 0;
}

int rtdev::RtRobot::set_robot_joint_max_limit(uint8_t joint, double joint_max_limit)
{
    // joint_para_[joint].max_pos_limit = joint_max_limit;
    // int data = (int)ceil(joint_max_limit);

    // if (robot_type_ == ROBOT_TYPE_AT)
    // {
    //     int dir = joint_servo_[joint]->get_dir();
    //     data = data * dir + joint_servo_[joint]->get_zero_offset();
    //     if (dir == -1)
    //     {
    //         // 7轴机型 限位处理 不放在伺服；由控制器完成
    //         return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_MIN_LIMIT, &data, [joint](uint8_t* rx_data, int s) -> int {
    //             if (s != 7)
    //                 return s;

    //             return 0;
    //         });
    //     }
    //     else
    //     {
    //         return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_MAX_LIMIT, &data, [joint](uint8_t* rx_data, int s) -> int {
    //             if (s != 7)
    //                 return s;

    //             return 0;
    //         });
    //     }
    // }
    // return joint_servo_[joint]->set_sdo(SERVO_OBJID_JOINT_MAX_LIMIT, &data, [joint](uint8_t* rx_data, int s) -> int {
    //     if (s != 7)
    //         return s;

    //     return 0;
    // });
    return 0;
}

int rtdev::RtRobot::query_robot_joint_collision_setting()
{
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     joint_servo_[i]->get_sdo(SERVO_OBJID_COLLISION_BASIC, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s < 4)
    //             return s;
    //         joint_servo_[i]->collision_base_threshold_ = rx_data[6] + (rx_data[5] << 8) + (rx_data[4] << 16) + (rx_data[3] << 24);
    //         return 0;
    //     });
    //     joint_servo_[i]->get_sdo(SERVO_OBJID_COLLISION_TH, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s < 4)
    //             return s;
    //         joint_servo_[i]->collision_torque_threshold_ = rx_data[6] + (rx_data[5] << 8) + (rx_data[4] << 16) + (rx_data[3] << 24);
    //         return 0;
    //     });
    // }
    return 0;
}

int rtdev::RtRobot::query_robot_joint_motor_supplier_id()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_SUPPLIER_ID, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->supplier_id_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_temperature()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_TEMPERATURE, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->temperature_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_average_power()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_AVERRAGE_POWER, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->average_power_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_current_fluctuation()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_CURRENT_FLUCTUAION, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->current_fluctuation_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_sum_running_circles()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_SUM_RUNNING_CYCLE, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->total_run_laps_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_sum_running_time()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_SUM_RUNNING_TIME, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->total_run_time_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_cur_running_circles()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_CUR_RUNNING_CYCLE, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->current_run_laps_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_cur_running_time()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_CUR_RUNNING_TIME, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->current_run_time_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_voltage()
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->get_sdo(SERVO_OBJID_VOLTAGE, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         joint_servo_[i]->voltage_ = get_data<int>(rx_data);
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::query_robot_joint_version()
{
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     joint_servo_[i]->get_sdo(
    //         SERVO_OBJID_SERVO_VERSION,
    //         [i, this](uint8_t* rx_data, int s) -> int {
    //             if (s < 4)
    //                 return s;
    //             joint_servo_[i]->version_ = rx_data[6] + (rx_data[5] << 8) + (rx_data[4] << 16) + (rx_data[3] << 24);
    //             return 0;
    //         },
    //         80,
    //         125);
    // }
    return 0;
}

int rtdev::RtRobot::query_robot_joint_first_poweron_date()
{
    const uint32_t kFirstPoweronDateSdoIndex_9013 = 90 << 8 | 13;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     joint_servo_[i]->get_sdo(SERVO_OBJID_POWER_ON_DATE, [i, this](uint8_t* rx_data, int s) -> int {
    //         if (s < 4)
    //             return s;
    //         joint_servo_[i]->first_poweron_date_ = rx_data[6] + (rx_data[5] << 8) + (rx_data[4] << 16) + (rx_data[3] << 24);
    //         return 0;
    //     });
    // }
    return 0;
}

// step: +1/-1
int rtdev::RtRobot::set_robot_joint_multi_turn_step(uint8_t joint, int step)
{
    // return joint_servo_[joint]->set_sdo(SERVO_OBJID_MULYI_TURN_STEP, &step, [](uint8_t* rx_data, int s) -> int {
    //     if (s != 7)
    //         return s;
    //     return 0;
    // });
    return 0;
}

int rtdev::RtRobot::set_robot_joint_torque_command_unit(int unit)  //0->current counts; 1->percents of rated torque
{
    int ret = 0;
    // for (size_t i = 0; i < joint_servo_.size(); i++)
    // {
    //     ret |= joint_servo_[i]->set_sdo(SERVO_OBJID_TORQUE_UNIT, &unit, [](uint8_t* rx_data, int s) -> int {
    //         if (s != 7)
    //             return s;
    //         return 0;
    //     });
    // }
    return ret;
}

int rtdev::RtRobot::set_robot_joint_avoid_pos_mutation(uint8_t joint, unsigned int para)
{
    // return joint_servo_[joint]->set_sdo(SERVO_OBJID_POSMUTATION, (int*)&para, [](uint8_t* rx_data, int s) -> int {
    //     if (s != 7)
    //         return s;
    //     return 0;
    // });
    return 0;
}
